package estrategia;

import java.io.IOException;
import java.util.ArrayList;
import javax.vecmath.Point3d;

import monitor.ManagerObjetos;
import comunicacion.Conector;

public class Historial {
	private static int TAMANIO_MAXIMO = 10;
	
	private Utiles utiles;
	private Conector entorno;
	
	private ArrayList<Point3d> posiciones;
	
	//Variables para detectar que murio el robot
	private double esfera_X;
	private double esfera_Y;
	private double angulo_barrido = 0;
	private int id;
	
	public Historial(Conector entorno, int id) throws IOException {
		utiles = new Utiles(entorno);
		this.entorno = entorno; 
		
		posiciones = new ArrayList<Point3d>();
		for (int i=0; i<10; i++) {
			posiciones.add(entorno.getCoordenadas(id));
		}
		
		if (id >= ParametrosGlobales.MI_ROBOT)
			id++;
		this.id = id;
	}
	
	public void limpiar() throws IOException {
		posiciones = new ArrayList<Point3d>();
		for (int i=0; i<10; i++) {
			posiciones.add(entorno.getCoordenadas(id));
		}
	}
	
	public void agregar(Point3d ultimaPosicion) throws IOException {
		posiciones.add(ultimaPosicion);
		
		if (posiciones.size() > TAMANIO_MAXIMO) {
			posiciones.remove(0);
		}				

		if (!ManagerObjetos.getInstance(null).estaEliminado(id)) {
			if(posiciones.size() == 1) {			
				esfera_X = ultimaPosicion.x;
				esfera_Y = ultimaPosicion.y;
				angulo_barrido = 0;			
			} else {
				
				double TOLERANCIA_ESFERA_MUERTE = 0.05; // distancia que puede variar el X y el Y
	
				/*Fijarse los cambios con respecto al Point3d anterior.*/
				
				//verifico que siga dentro de la esfera, sino -> reset
				if (utiles.distanciaEuclidea(posiciones.get(posiciones.size()-2), ultimaPosicion) >= TOLERANCIA_ESFERA_MUERTE) {
					
					resetearDatosMuerte();
					
				//verifico la direccion de giro, si antihoraria -> reset (o la misma pos sino reset)
				} else if(giroAntihorario()){
					
					resetearDatosMuerte();
					
				} else {
					//Actualizo en angulo de barrido
					double angulo_anterior = posiciones.get(posiciones.size()-2).z;
					
					if(giroAntihorarioAux(angulo_anterior, ultimaPosicion.z))
						angulo_barrido = angulo_barrido - utiles.getDiferenciaAngulos(angulo_anterior,ultimaPosicion.z);
					else
						angulo_barrido = angulo_barrido + utiles.getDiferenciaAngulos(angulo_anterior,ultimaPosicion.z);
					
					if(angulo_barrido > (4*Math.PI)) {
						//Murio el robot
						//TODO: Matar al robot, usando el atrubuto id que me pasan en el contructor
														 
						try {
							//Se que el getInstance fue invocado previamente, por lo que no interesa su atributo
							ManagerObjetos managerObjetos = ManagerObjetos.getInstance(null);
							managerObjetos.deshabilitarEjercitoDerrotado(id);
							System.out.println("Murio el robot "+ id);
						} catch (IOException e) {
							// TODO Auto-generated catch block
							e.printStackTrace();
						}
						
					}
				}			
			}
		}
	}
	
	
	public Point3d getUltimaPosicion() {
		return posiciones.get(posiciones.size()-1);
	}
	
	public double getVelocidadPromedio() {
		int intervalos = posiciones.size() - 1;		
		if (intervalos < 1) {
			return 0;
		} 
		
		double distanciaAcumulada = 0;
		
		for (int i=1; i<posiciones.size(); i++) {
			// No consideramos el intervalo de tiempo porque podemos considerarlo fijo
			double distanciaIntervaloActual = utiles.distanciaEuclidea(posiciones.get(i-1), posiciones.get(i));
			
			distanciaAcumulada += distanciaIntervaloActual;
		}
		
		// En lugar de considerar tiempo, que de hecho es constante, consideramos "pasos"
		double tiempoTotal = (double)intervalos;
		double velocidadPromedio = distanciaAcumulada / tiempoTotal;
		//System.out.println("Velocidad Promedio = " + velocidadPromedio);
		
		return velocidadPromedio;
	}
	
	public double getRotacionPromedio() {
		int cantMedidas = posiciones.size();		
		if (cantMedidas < 1) {
			return 0;
		} 
		
		double rotacionAcumulada = 0;
		
		for (int i=0; i<posiciones.size(); i++) {
			double rotacionActual = posiciones.get(i).z;
			
			rotacionAcumulada += rotacionActual;
		}	
		
		double rotacionPromedio = rotacionAcumulada / (double)cantMedidas;
		//System.out.println("Rotacion Promedio = " + rotacionPromedio);
		
		return rotacionPromedio;
	}
	
		
	private void resetearDatosMuerte() {
			
		esfera_X = getUltimaPosicion().x;
		esfera_Y = getUltimaPosicion().y;
		angulo_barrido = 0;
	}
	
	
	/**
	 * Retorna true si los 4 barridos fueron antihorarios
	 * */
	private boolean giroAntihorario() {
		
		if(posiciones.size() <= 1) {
			return false;
		}
		
		boolean res = true;
		
		for (int i=posiciones.size()-5; i < posiciones.size(); i++) {
			res = res && giroAntihorarioAux(posiciones.get(i-1).z,posiciones.get(i).z);
		}
		
		return res;
	} 
	
	private boolean giroAntihorarioAux(double p1, double p2) {
		
		double angulo_A = p1 + (Math.PI/2);
		double angulo_B = p1 - (Math.PI/2);
		return utiles.getDiferenciaAngulos(angulo_A, p2) < utiles.getDiferenciaAngulos(angulo_B, p2); 
	}
}
